/*
 * GPSService.cpp
 *
 *  Created on: Aug 14, 2014
 *      Author: Cameron Karlsson
 *
 *  See the license file included with this source.
 */


#include <NMEA_PARSER/GPSService.h>
#include <NMEA_PARSER/NumberConversion.h>

#include <iostream>
#include <cmath>

using namespace std;
using namespace std::chrono;

using namespace nmea;

// ------ Some helpers ----------
// Takes the NMEA lat/long format (dddmm.mmmm, [N/S,E/W]) and converts to degrees N,E only
// 将字符串的经纬度转成角度（每次转一个，经度或者纬度）
double convertLatLongToDeg(string llstr, string dir){

    double pd = parseDouble(llstr);
    double deg = trunc(pd / 100);				//get ddd from dddmm.mmmm
    double mins = pd - deg * 100;

    deg = deg + mins / 60.0;

    char hdg = 'x';
    if (!dir.empty()){
        hdg = dir[0];
    }

    //everything should be N/E, so flip S,W
    if (hdg == 'S' || hdg == 'W'){
        deg *= -1.0;
    }

    return deg;
}

// 单位： 节 ====> km/h
double convertKnotsToKilometersPerHour(double knots){
    return knots * 1.852;
}



// ------------- GPSSERVICE CLASS -------------
// 构造函数
GPSService::GPSService(NMEAParser& parser) {
    attachToParser(parser);		// attach to parser in the GPS object
}

GPSService::~GPSService() {
    // TODO Auto-generated destructor stub
}

void GPSService::attachToParser(NMEAParser& _parser){

    // http://www.gpsinformation.org/dale/nmea.htm
    /* used sentences...
    $GPGGA		- time,position,fix data
    $GPGSA		- gps receiver operating mode, satellites used in position and DOP values
    $GPGSV		- number of gps satellites in view, satellite ID, elevation,azimuth, and SNR
    $GPRMC		- time,date, position,course, and speed data
    $GPVTG		- course and speed information relative to the ground
    $GPZDA		- 1pps timing message
    $PSRF150	- gps module "ok to send"
    */

    // 预设的NMEA语句处理
    // 重点
    _parser.setSentenceHandler("GPGGA", [this](const NMEASentence& nmea){
        return this->read_GPGGA(nmea);
    });

    _parser.setSentenceHandler("GTIMU", [this](const NMEASentence& nmea){
        return this->read_GTIMU(nmea);
    });

    _parser.setSentenceHandler("GPFPD", [this](const NMEASentence& nmea){
        return this->read_GPFPD(nmea);
    });

    _parser.setSentenceHandler("NVSTD", [this](const NMEASentence& nmea){
        return this->read_NVSTD(nmea);
    });

    //	_parser.setSentenceHandler("GPGSA", [this](const NMEASentence& nmea){
    //		this->read_GPGSA(nmea);
    //	});
    //	_parser.setSentenceHandler("GPGSV", [this](const NMEASentence& nmea){
    //		this->read_GPGSV(nmea);
    //	});
    //	_parser.setSentenceHandler("GPRMC", [this](const NMEASentence& nmea){
    //		this->read_GPRMC(nmea);
    //	});
    //	_parser.setSentenceHandler("GPVTG", [this](const NMEASentence& nmea){
    //		this->read_GPVTG(nmea);
    //	});

}

// 解析GPGGA语句
bool GPSService::read_GPGGA(const NMEASentence& nmea){
    /* -- EXAMPLE --
    $GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47

    $GPGGA,205630.945,3346.1070,N,08423.6687,W,0,03,,30.8,M,-30.8,M,,0000*73		// ATLANTA!!!!

    Where:
    GGA          Global Positioning System Fix Data
    index:
    [0] 123519       Fix taken at 12:35:19 UTC
    [1-2] 4807.038,N   Latitude 48 deg 07.038' N
    [3-4] 1131.000,E  Longitude 11 deg 31.000' E
    [5] 1            Fix quality: 0 = invalid
    1 = GPS fix (SPS)
    2 = DGPS fix
    3 = PPS fix
    4 = Real Time Kinematic
    5 = Float RTK
    6 = estimated (dead reckoning) (2.3 feature)
    7 = Manual input mode
    8 = Simulation mode
    [6] 08           Number of satellites being tracked
    [7] 0.9          Horizontal dilution of position
    [8-9] 545.4,M      Altitude, Meters, above mean sea level
    [10-11] 46.9,M       Height of geoid (mean sea level) above WGS84
    ellipsoid
    [12] (empty field) time in seconds since last DGPS update
    [13] (empty field) DGPS station ID number
    [13]  *47          the checksum data, always begins with *
    */

    RTK_STATUS rtk_;

    try
    {
        if (!nmea.checksumOK()){
            //throw NMEAParseError("Checksum is invalid!");
            std::cout<<"Checksum is invalid!"<<std::endl;
            return false;
        }

//        if (nmea.parameters.size() < 14){
//            //throw NMEAParseError("GPS data is missing parameters.");
//            std::cout<<"Checksum is invalid!"<<std::endl;
//            return false;
//        }


        // TIMESTAMP
        //		this->fix.timestamp.setTime(parseDouble(nmea.parameters[0]));

        string sll;
        string dir;
        // LAT
        sll = nmea.parameters[1];
        dir = nmea.parameters[2];
        if (!sll.empty()){
            //			this->fix.latitude = convertLatLongToDeg(sll, dir);
            rtk_.latitude=convertLatLongToDeg(sll, dir);
        }

        // LONG
        sll = nmea.parameters[3];
        dir = nmea.parameters[4];
        if (!sll.empty()){
            //			this->fix.longitude = convertLatLongToDeg(sll, dir);
            rtk_.longitude=convertLatLongToDeg(sll, dir);
        }


        // FIX QUALITY
        bool lockupdate = false;
        rtk_.fix_type=(uint8_t)parseInt(nmea.parameters[5]);
        //		this->fix.quality = (uint8_t)parseInt(nmea.parameters[5]);
        //		if (this->fix.quality == 0){
        //			lockupdate = this->fix.setlock(false);
        //		}
        //		else if (this->fix.quality == 1){
        //			lockupdate = this->fix.setlock(true);
        //		}
        //		else {}


        // TRACKING SATELLITES
        //		this->fix.trackingSatellites = (int32_t)parseInt(nmea.parameters[6]);
        //		if (this->fix.visibleSatellites < this->fix.trackingSatellites){
        //			this->fix.visibleSatellites = this->fix.trackingSatellites;		// the visible count is in another sentence.
        //		}
        //TODO : maybe over 255？
        rtk_.satellites_num=(uint8_t)parseInt(nmea.parameters[6]);

        // ALTITUDE
        if (!nmea.parameters[8].empty()){
            //			this->fix.altitude = parseDouble(nmea.parameters[8]);
            rtk_.altitude=parseDouble(nmea.parameters[8]);
        }
        else {
            // leave old value
        }

        rtk_.hdop=parseDouble(nmea.parameters[7]);
        // 计算标准差
        rtk_.lat_std=(rtk_.hdop * epe_quality[rtk_.fix_type]);
        rtk_.lon_std=(rtk_.hdop * epe_quality[rtk_.fix_type]);
        rtk_.alti_std=(4 * rtk_.hdop * epe_quality[rtk_.fix_type]);

//        //calling handlers
//        if (lockupdate){
//            //			this->onLockStateChanged(this->fix.haslock);
//        }
//        this->onUpdate();

        rtk_.updated=true;
        this->gps_=rtk_;
        return true;
    }
    catch (NumberConversionError& ex)
    {
        NMEAParseError pe("GPS Number Bad Format [$GPGGA] :: " + ex.message, nmea);
        throw pe;
    }
    catch (NMEAParseError& ex)
    {
        NMEAParseError pe("GPS Data Bad Format [$GPGGA] :: " + ex.message, nmea);
        throw pe;
    }
}


bool GPSService::read_GPRMC(const NMEASentence& nmea){
    /*  -- EXAMPLE ---
    $GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A
    $GPRMC,235957.025,V,,,,,,,070810,,,N*4B
    $GPRMC,061425.000,A,3346.2243,N,08423.4706,W,0.45,18.77,060914,,,A*47

    Where:
    RMC          Recommended Minimum sentence C
    [0] 123519       Fix taken at 12:35:19 UTC
    [1] A            Status A=active or V=Void.
    [2-3] 4807.038,N   Latitude 48 deg 07.038' N
    [4-5] 01131.000,E  Longitude 11 deg 31.000' E
    [6] 022.4        Speed over the ground in knots
    [7] 084.4        Track angle in degrees True
    [8] 230394       Date - 23rd of March 1994
    [9-10] 003.1,W      Magnetic Variation
    [10] *6A          The checksum data, always begins with *
    // NMEA 2.3 includes another field after
    */

    //	try
    //	{
    //		if (!nmea.checksumOK()){
    //			throw NMEAParseError("Checksum is invalid!");
    //		}
    //
    //		if (nmea.parameters.size() < 11){
    //			throw NMEAParseError("GPS data is missing parameters.");
    //		}
    //
    //		// TIMESTAMP
    //		this->fix.timestamp.setTime(parseDouble(nmea.parameters[0]));
    //
    //		string sll;
    //		string dir;
    //		// LAT
    //		sll = nmea.parameters[2];
    //		dir = nmea.parameters[3];
    //		if (!sll.empty()){
    //			this->fix.latitude = convertLatLongToDeg(sll, dir);
    //		}
    //
    //		// LONG
    //		sll = nmea.parameters[4];
    //		dir = nmea.parameters[5];
    //		if (!sll.empty()){
    //			this->fix.longitude = convertLatLongToDeg(sll, dir);
    //		}
    //
    //
    //		// ACTIVE
    //		bool lockupdate = false;
    //		char status = 'V';
    //		if (!nmea.parameters[1].empty()){
    //			status = nmea.parameters[1][0];
    //		}
    //		this->fix.status = status;
    //		if (status == 'V'){
    //			lockupdate = this->fix.setlock(false);
    //		}
    //		else if (status == 'A') {
    //			lockupdate = this->fix.setlock(true);
    //		}
    //		else {
    //			lockupdate = this->fix.setlock(false);		//not A or V, so must be wrong... no lock
    //		}
    //
    //
    //		this->fix.speed = convertKnotsToKilometersPerHour(parseDouble(nmea.parameters[6]));		// received as knots, convert to km/h
    //		this->fix.travelAngle = parseDouble(nmea.parameters[7]);
    //		this->fix.timestamp.setDate((int32_t)parseInt(nmea.parameters[8]));
    //
    //
    //		//calling handlers
    //		if (lockupdate){
    //			this->onLockStateChanged(this->fix.haslock);
    //		}
    //		this->onUpdate();
    //	}
    //	catch (NumberConversionError& ex)
    //	{
    //		NMEAParseError pe("GPS Number Bad Format [$GPRMC] :: " + ex.message, nmea);
    //		throw pe;
    //	}
    //	catch (NMEAParseError& ex)
    //	{
    //		NMEAParseError pe("GPS Data Bad Format [$GPRMC] :: " + ex.message, nmea);
    //		throw pe;
    //	}
    return  true;
}


bool GPSService::read_GTIMU(const NMEASentence &nmea)
{
    /* -- EXAMPLE --
    $GTIMU,2106,113633.540,-0.3147,-0.0638,0.1615,-0.0087,-0.0025,0.9989,51.0*6D

    Where:
    IMU          IMU Data
    index:
    [0] 2016       GPS周（1980年1月6日凌晨之间0点起）   [wwww]
    [1] 113633.540  星期内的秒数                      [ssssss.sss]
    [2] -0.3147     GYRO_X (度/秒)
    [3] -0.0638     GYRO_Y
    [4] 0.1615      GYRO_Z
    [5] -0.0087     ACC_X
    [6] -0.0025     ACC_Y
    [7] 0.9989      ACC_Z
    [8] 51.0        Tpr(温度： 摄氏度)
    [9] *6D         the checksum data, always begins with *
    */

    //INS_STATUS imu__;

    try
    {
        if (!nmea.checksumOK()){
            //throw NMEAParseError("Checksum is invalid!");
            std::cout<<"Checksum is invalid!"<<std::endl;
            return false;
        }

//        if (nmea.parameters.size() < 14){
//            //throw NMEAParseError("GPS data is missing parameters.");
//            std::cout<<"Checksum is invalid!"<<std::endl;
//            return false;
//        }


        // TIMESTAMP
        //		this->fix.timestamp.setTime(parseDouble(nmea.parameters[0]));


        // LAT
        double gyro_x = deg2rad(parseDouble(nmea.parameters[2]));
        double gyro_y = deg2rad(parseDouble(nmea.parameters[3]));
        double gyro_z = deg2rad(parseDouble(nmea.parameters[4]));
        if (!std::isnan(gyro_x)&&!std::isnan(gyro_y)&&!std::isnan(gyro_z)){
            //			this->fix.latitude = convertLatLongToDeg(sll, dir);
            imu_.gyro_x=gyro_x;
            imu_.gyro_y=gyro_y;
            imu_.gyro_z=gyro_z;

            // NEW
            ins_.gyro_x=gyro_x;
            ins_.gyro_y=gyro_y;
            ins_.gyro_z=gyro_z;
        }

        double acc_x=parseDouble(nmea.parameters[5])*gravity;
        double acc_y=parseDouble(nmea.parameters[6])*gravity;
        double acc_z=parseDouble(nmea.parameters[7])*gravity;
        if(!std::isnan(acc_x)&&!std::isnan(acc_y)&&!std::isnan(acc_z)){
            imu_.acc_x=acc_x;
            imu_.acc_y=acc_y;
            imu_.acc_z=acc_z;
        }

        double tpr=parseDouble(nmea.parameters[8]);
        if(!std::isnan(tpr)){
            imu_.dev_temperature=tpr;
        }

        // FIX QUALITY
        bool lockupdate = false;

        //calling handlers
        if (lockupdate){
            //			this->onLockStateChanged(this->fix.haslock);
        }
        this->onUpdate();

        //        this->gps_=rtk_;
        imu_.updated=true;
        //this->imu_=imu__;
        return true;
    }
    catch (NumberConversionError& ex)
    {
        NMEAParseError pe("GPS Number Bad Format [$GPGGA] :: " + ex.message, nmea);
        throw pe;
    }
    catch (NMEAParseError& ex)
    {
        NMEAParseError pe("GPS Data Bad Format [$GPGGA] :: " + ex.message, nmea);
        throw pe;
    }
}

// ZXY欧拉角转四元数
void eulerToQuaternion(double roll_,double pitch_,double yaw_ ,
                       double &qx, double &qy, double &qz,double &qw){
    double coeff =0.5;
    double r = roll_ * coeff;
    double p = pitch_ * coeff;
    double y = yaw_ * coeff;

    double sr = sin(r);
    double sp = sin(p);
    double sy = sin(y);

    double cr = cos(r);
    double cp = cos(p);
    double cy = cos(y);

    qw = cr * cp * cy - sr * sp * sy;
    qx = cr * sp * cy - sr * cp * sy;
    qy = cr * sp * sy + sr * cp * cy;
    qz = cr * cp * sy + sr * sp * cy;
    if (qw < 0.0){
        qw=-qw;
        qx=-qx;
        qy=-qy;
        qz=-qz;
    }
}

bool GPSService::read_GPFPD(const NMEASentence &nmea)
{
    /* -- EXAMPLE --
    $GPFPD,2106,113939.120,0.000,-0.177,0.467,29.81053089,121.55182266,37.10,-0.054,-0.133,0.249,0.000,0,7,00*6B

    Where:
    IMU          IMU Data
    index:
    [0] 2016            GPS周（1980年1月6日凌晨之间0点起）   [wwww]
    [1] 113939.120      星期内的秒数                      [ssssss.sss]
    [2] 0.000           偏航角(度)  (0~359.99) 顺时针为正
    [3] -0.177          俯仰角pitch (-90~90)   逆时针为正
    [4] 0.467           横滚角roll  (-180~180) 逆时针为正
    [5] 29.81053089     纬度        (-90~90)   （+北纬，-南半球）
    [6] 121.55182266    经度        (-180~180) （+东经，-西经）
    [7] 37.10           高度        (米)
    [8] -0.054          东向速度     (米/秒)
    [9] -0.133          北向速度
    [10] 0.249          天向速度
    [11] 0.000          基线长度
    [12] 0              天线1卫星数
    [13] 7              天线2卫星数
    [14] 00             状态  （4B为RTK FIX）
    高半字节ASCII码:
        0: GPS
        2: 伪距差分
        4: RTK FIX
        5: RTK FLOAT
    低半字节ASCII码：
        0: 初始化
        1: 粗对准
        2: 精对准
        3: GPS定位
        4: GPS定向
        5: RTK定位
        6: DMI组合
        7: DMI标定
        8: 纯惯导
        9: 零速校正
        A: VG模式
        B: RTK定向
        C: 进入导航初始化
    [15] *6B         the checksum data, always begins with *
    */

    //INS_STATUS ins_;

    try
    {
        if (!nmea.checksumOK()){
            //throw NMEAParseError("Checksum is invalid!");
            std::cout<<"Checksum is invalid!"<<std::endl;
            return false;
        }

//        if (nmea.parameters.size() < 14){
//            //throw NMEAParseError("GPS data is missing parameters.");
//            std::cout<<"Checksum is invalid!"<<std::endl;
//            return false;
//        }

        // qpc 0806
        // RPY
        double yaw_ = -NormalizeAngle(deg2rad(parseDouble(nmea.parameters[2])));
        double pitch_ = deg2rad(parseDouble(nmea.parameters[3]));
        double roll_ = deg2rad(parseDouble(nmea.parameters[4]));
        if (!std::isnan(yaw_)&&!std::isnan(pitch_)&&!std::isnan(roll_)){
            ins_.yaw=yaw_;
            ins_.pitch=pitch_;
            ins_.roll=roll_;

            double qw=1,qx=0,qy=0,qz=0;
            eulerToQuaternion(roll_,pitch_,yaw_,qx,qy,qz,qw);
            imu_.qw=qw;
            imu_.qx=qx;
            imu_.qy=qy;
            imu_.qz=qz;
        }


        static size_t fix4B_times=0;
        static bool init_success=false;

        // 先检查状态
        std::string ins_status__= nmea.parameters[14];
        if(ins_status__=="4B" && !init_success ){
            if(fix4B_times<=10){
                fix4B_times++;
                return  false;
            }else{
                init_success=true;
                fix4B_times=0;
            }
        }


        if(!init_success){
            return false;
        }

        ins_.fix_type=(uint8_t)parseInt(ins_status__,16);

        // TIMESTAMP
        //		this->fix.timestamp.setTime(parseDouble(nmea.parameters[0]));
/*
        // RPY
        double yaw_ = -NormalizeAngle(deg2rad(parseDouble(nmea.parameters[2])));
        double pitch_ = deg2rad(parseDouble(nmea.parameters[3]));
        double roll_ = deg2rad(parseDouble(nmea.parameters[4]));
        if (!std::isnan(yaw_)&&!std::isnan(pitch_)&&!std::isnan(roll_)){
            ins_.yaw=yaw_;
            ins_.pitch=pitch_;
            ins_.roll=roll_;

            double qw=1,qx=0,qy=0,qz=0;
            eulerToQuaternion(roll_,pitch_,yaw_,qx,qy,qz,qw);
            imu_.qw=qw;
            imu_.qx=qx;
            imu_.qy=qy;
            imu_.qz=qz;
        }
*/

        string sll;
        // LAT
        sll = nmea.parameters[5];
        if (!sll.empty()){
            ins_.latitude=parseDouble(nmea.parameters[5]);
        }

        // LONG
        sll = nmea.parameters[6];
        if (!sll.empty()){
            //			this->fix.longitude = convertLatLongToDeg(sll, dir);
            ins_.longitude=parseDouble(nmea.parameters[6]);
        }

        // ALti
        double altitude__=parseDouble(nmea.parameters[7]);
        if(!std::isnan(altitude__)){
            ins_.altitude=altitude__;
        }

        double ve_ = parseDouble(nmea.parameters[8]);
        double vn_ = parseDouble(nmea.parameters[9]);
        double vu_ = parseDouble(nmea.parameters[10]);
        if (!std::isnan(ve_)&&!std::isnan(vn_)&&!std::isnan(vu_)){
            ins_.vel_e=ve_;
            ins_.vel_n=vn_;
            ins_.vel_u=vu_;
        }

        double sattellites_nums=parseDouble(nmea.parameters[12])+parseDouble(nmea.parameters[13]);
        if(!std::isnan(sattellites_nums)){
            ins_.satellites_num=sattellites_nums;
        }

        // FIX QUALITY
        bool lockupdate = false;

        //calling handlers
        if (lockupdate){
            //			this->onLockStateChanged(this->fix.haslock);
        }
        this->onUpdate();
        ins_.updated=true;

        //        this->gps_=rtk_;
        return true;
    }
    catch (NumberConversionError& ex)
    {
        NMEAParseError pe("GPS Number Bad Format [$GPGGA] :: " + ex.message, nmea);
        throw pe;
    }
    catch (NMEAParseError& ex)
    {
        NMEAParseError pe("GPS Data Bad Format [$GPGGA] :: " + ex.message, nmea);
        throw pe;
    }
}

bool GPSService::read_NVSTD(const NMEASentence &nmea)
{
    /* -- EXAMPLE --
    $NVSTD,2106,114792.000,0.00,0.00,-13450675326710152000000000000000000.00,6382025699198825600000.0000000,0.0000000,-234758558231502680000000000000000.00,0.000,0.000,0.000*73

    Where:
    STD          INS的标准差
    index:
    [0] 2016       GPS周（1980年1月6日凌晨之间0点起）   [wwww]
    [1] 114792.000  星期内的秒数                      [ssssss.sss]
    [2] 1           偏航角标准差 (度)
    [3] 1           俯仰角标准差 (度)
    [4] 1           横滚角标准差 (度)
    [5] -0.0087     纬度标准差
    [6] -0.0025     经度标准差
    [7] -0.0025     高度标准差
    [8] 0.001       东向速度标准差
    [9] 0.001       北向速度标准差
    [10] 0.001       天向速度标准差
    [11] *6D         the checksum data, always begins with *
    */

    try
    {
        if (!nmea.checksumOK()){
            //throw NMEAParseError("Checksum is invalid!");
            std::cout<<"Checksum is invalid!"<<std::endl;
            return false;
        }

//        if (nmea.parameters.size() < 14){
//            //throw NMEAParseError("GPS data is missing parameters.");
//            std::cout<<"Checksum is invalid!"<<std::endl;
//            return false;
//        }


        // TIMESTAMP
        //		this->fix.timestamp.setTime(parseDouble(nmea.parameters[0]));

        double yaw_STD=deg2rad(parseDouble(nmea.parameters[2]));
        double pitch_STD=deg2rad(parseDouble(nmea.parameters[3]));
        double roll_STD=deg2rad(parseDouble(nmea.parameters[4]));
        if(!std::isnan(yaw_STD)&&!std::isnan(pitch_STD)&&!std::isnan(roll_STD)){
            ins_.yaw_std=yaw_STD;
            ins_.pitch_std=pitch_STD;
            ins_.roll_std=roll_STD;
        }

        double lat_STD=parseDouble(nmea.parameters[5]);
        double lon_STD=parseDouble(nmea.parameters[6]);
        double alt_STD=parseDouble(nmea.parameters[7]);
        if(!std::isnan(lat_STD)&&!std::isnan(lon_STD)&&!std::isnan(alt_STD)){
            ins_.lat_std=lat_STD;
            ins_.lon_std=lon_STD;
            ins_.alti_std=alt_STD;
        }

        double ve_STD=parseDouble(nmea.parameters[8]);
        double vn_STD=parseDouble(nmea.parameters[9]);
        double vu_STD=parseDouble(nmea.parameters[10]);
        if(!std::isnan(ve_STD)&&!std::isnan(vn_STD)&&!std::isnan(vu_STD)){
            ins_.ve_std=ve_STD;
            ins_.vn_std=vn_STD;
            ins_.vu_std=vu_STD;
        }

        // FIX QUALITY
        bool lockupdate = false;

        //calling handlers
        if (lockupdate){
            //			this->onLockStateChanged(this->fix.haslock);
        }
        this->onUpdate();

        //        this->gps_=rtk_;
        return true;
    }
    catch (NumberConversionError& ex)
    {
        NMEAParseError pe("GPS Number Bad Format [$GPGGA] :: " + ex.message, nmea);
        throw pe;
    }
    catch (NMEAParseError& ex)
    {
        NMEAParseError pe("GPS Data Bad Format [$GPGGA] :: " + ex.message, nmea);
        throw pe;
    }
}
